/**
  ******************************************************************************
  * Copyright (c) 2022 - ~, SCUT-RobotLab Development Team
  * @file    Hardware_Node.h
  * @author   
  * @brief   
  ******************************************************************************
  */
#ifndef HARDWARE_NODE_H
#define HARDWARE_NODE_H

/* Includes ------------------------------------------------------------------*/
#include "ros/ros.h"
#include "common/Driver/rc_usart.h"
#include "common/RobotCommand.h"
#include "common/LegData.h"
#include "sensor_msgs/Imu.h"
#include "std_msgs/Int8.h"
/* Private macros ------------------------------------------------------------*/
/* Private type --------------------------------------------------------------*/
/* Exported function declarations --------------------------------------------*/	

class Hardware_Node{
public:
  Hardware_Node(ros::NodeHandle* _n, std::string rc_port1, std::string rc_port2): 
  n(_n), rc_usart(rc_port1, rc_port2){
      Node_Init();
  }
  
  void Run(void);

private:
  ros::NodeHandle* n;
  ros::Subscriber RobotCmd_Suber;
  ros::Subscriber IMUData_Suber;
  ros::Subscriber AU_Suber;
  ros::Publisher  LegData_Puber;
  ros::Publisher  ImuData_Puber;
  RC_Usart rc_usart;
  RobotCommand _cmd;

  void Node_Init(void);
  void  RobotCmd_Callback(const common::RobotCommandConstPtr& msg);
  void  IMUData_Callback(const sensor_msgs::ImuConstPtr& msg);
  void AU_Callback(const std_msgs::Int8ConstPtr& msg);
  void Publish_LegData(LegData* data);
  bool Cmd_Available(const RobotCommand &cmd);
};

#endif
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/
 

